package jia;

import jason.asSemantics.DefaultInternalAction;
import jason.asSemantics.TransitionSystem;
import jason.asSemantics.Unifier;
import jason.asSyntax.Atom;
import jason.asSyntax.NumberTerm;
import jason.asSyntax.Term;

import java.awt.Point;
import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.HashSet;
import java.util.List;
import java.util.PriorityQueue;
import java.util.TreeSet;
import java.util.logging.Level;

import prop.PropertiesReader;

import arch.CowboyArch;
import arch.LocalWorldModel;
import env.WorldModel;

public class astar extends DefaultInternalAction {
	
	public LocalWorldModel model = null;
	
    Node[][] nodes = null;
    
	public astar() {
	}
	
    @Override
    public Object execute(TransitionSystem ts, Unifier un, Term[] args) throws Exception {
        try { 
        	long start = System.currentTimeMillis();
            int myX = (int)((NumberTerm)args[0]).solve();
			int myY = (int)((NumberTerm)args[1]).solve();
			int targetX = (int)((NumberTerm)args[2]).solve();
			int targetY = (int)((NumberTerm)args[3]).solve();
			
			/*
			//If dist is large, dont use AStar since it is costly
			if(Math.abs(myX - targetX) + Math.abs(myY - targetY) > 30){				
				return un.unifies(args[4], new Atom("skip"));
			}
			*/
			
			
			Solution sol = null;
			
			if(args.length == 6)
			{
				sol = AStarSearch.shortestPath(myX, myY, targetX, targetY, ts, false);
			}
			else
			{
				sol = AStarSearch.search(myX, myY, targetX, targetY, ts, false);
			}
			String action = "skip";
			
			if(sol.succeded()){
				action = sol.getFirstMove();
			}			
			long end = System.currentTimeMillis();
			delegate_target.t1 += end-start;
			return un.unifies(args[4], new Atom(action));
            
        } catch (Throwable e) {
            ts.getLogger().log(Level.SEVERE, "astar error: "+e, e);
        }
        return false;
    } // execute
    
    public static ArrayList<Node> getShortestPath(int startX, int startY, int endX, int endY,
    		TransitionSystem ts, boolean fenceClear){
    	//long start = System.currentTimeMillis();
    	ArrayList<Node> temp = AStarSearch.shortestPath(startX, startY, endX, endY, ts, fenceClear).getPath();
		//long end = System.currentTimeMillis();
		//delegate_target.t1 += end-start;
    	return temp;
    }
    
    //Returns the true shortest path without punishments
    public static Solution getShortestPath2(int startX, int startY, int endX, int endY,
    		TransitionSystem ts, boolean fenceClear){
    	//long start = System.currentTimeMillis();
    	Solution temp = AStarSearch.trueShortestPath(startX, startY, endX, endY, ts, fenceClear);   
		//long end = System.currentTimeMillis();
		//delegate_target.t1 += end-start;
    	return temp;
    }
}

class AStarSearch {
	
	//Returns shortest path with punishment for herders
	public static Solution search(int startX, int startY,
			int endX, int endY, TransitionSystem ts, boolean fenceClear){
		
    	CowboyArch arch = (CowboyArch)ts.getUserAgArch();
        LocalWorldModel model = arch.getModel();
		
        Heuristic h = new Heuristic(model, endX, endY);
        
		HashSet<Node> closed = new HashSet<Node>();
		PriorityQueue<Node> fringe = new PriorityQueue<Node>(50,h);

		
		Node initialNode = new Node(startX, startY, null, 0, h);
		fringe.add(initialNode);

		while(true){	
			if(fringe.isEmpty()){
				return new Solution();
			}
			Node n = fringe.poll();
			if(n.getX() == endX && n.getY() == endY)
				return new Solution(initialNode, n);
			
			if(!(closed.contains(n))){
				closed.add(n);
				List<Node> children = n.expand(model, Node.PUNISHMENT, fenceClear);
				fringe.addAll(children);
			}			
		}
	}
	
	//Returns the shortest path with special punishment for attackers
	public static Solution shortestPath(int startX, int startY,
			int endX, int endY, TransitionSystem ts, boolean fenceClear){
		
    	CowboyArch arch = (CowboyArch)ts.getUserAgArch();
        LocalWorldModel model = arch.getModel();
		
        Heuristic h = new Heuristic(model, endX, endY);
        
		HashSet<Node> closed = new HashSet<Node>();
		PriorityQueue<Node> fringe = new PriorityQueue<Node>(50,h);

		
		Node initialNode = new Node(startX, startY, null, 0, h);
		fringe.add(initialNode);

		while(true){	
			if(fringe.isEmpty()){
				return new Solution();
			}
			Node n = fringe.poll();
			if(n.getX() == endX && n.getY() == endY)
				return new Solution(initialNode, n);
			
			if(!(closed.contains(n))){
				closed.add(n);
				List<Node> children = n.expand(model, Node.PUNISHMENT_SPECIAL, fenceClear);
				fringe.addAll(children);
			}			
		}		
	}
	
	//Returns the true shortest path
	public static Solution trueShortestPath(int startX, int startY,
			int endX, int endY, TransitionSystem ts, boolean fenceClear){
		
    	CowboyArch arch = (CowboyArch)ts.getUserAgArch();
        LocalWorldModel model = arch.getModel();
		
        Heuristic h = new Heuristic(model, endX, endY);
        
		HashSet<Node> closed = new HashSet<Node>();
		PriorityQueue<Node> fringe = new PriorityQueue<Node>(50,h);

		
		Node initialNode = new Node(startX, startY, null, 0, h);
		fringe.add(initialNode);

		while(true){	
			if(fringe.isEmpty()){
				return new Solution();
			}
			Node n = fringe.poll();
			if(n.getX() == endX && n.getY() == endY)
				return new Solution(initialNode, n);
			
			if(!(closed.contains(n))){
				closed.add(n);
				List<Node> children = n.expand(model, Node.NO_PUNISHMENT, fenceClear);
				fringe.addAll(children);
			}			
		}		
	}
} //AStarSearch

class Node{
	
	private int x,y;
	private Node parent;
	private Heuristic h;
	private int g;
	private int f;
	
	public Node(int x, int y, Node p, int g, Heuristic h){
		this.parent = p;
		this.g = g;
		this.h = h;
		this.x = x;
		this.y = y;
		f = g + h.heuristic(this);
	}
	
	public Node getParent(){
		return parent;
	}
	
	public static final int PUNISHMENT = 0;
	public static final int PUNISHMENT_SPECIAL = 1;
	public static final int NO_PUNISHMENT = 2;
	
	public List<Node> expand(LocalWorldModel model, int punishment, boolean fenceClear){
		List<Node> children = new ArrayList<Node>();		
		for(int i = -1; i <= 1; i++)
			for(int j = -1; j <= 1; j++)
				if(!(i == 0 && j == 0) &&
						i+x >= 0 && j+y >= 0 &&
						i+x < model.getWidth() && j+y < model.getHeight() &&
						isClear(x+i,y+j,model,fenceClear)){		
					int newG = punishment(model,x+i,y+j);
					int newGSpecial = punishmentSpecial(model,x+i,y+j);
					Node child;
					if(punishment == PUNISHMENT)
						child = new Node(i+x,j+y,this,this.g + newG + 1,h);
					else if(punishment == PUNISHMENT_SPECIAL)
						child = new Node(i+x,j+y,this,this.g + newGSpecial + 1,h);
					else
						child = new Node(i+x,j+y,this,this.g + 1,h);						
					children.add(child);
				}
		return children;
    }

	private int punishment(LocalWorldModel model, int x, int y){
		int p = 0;
		
		Point cor = new Point(model.getCorralCenter().x, model.getCorralCenter().y);
		int xDir = x - cor.x;
		int yDir = y - cor.y;
		
		
		for(int i = -5; i <= 5; i++){
			for(int j = -5; j <= 5; j++){
				if(i+x >= 0 && i+x < model.getWidth() &&
					j+y >= 0 && j+y < model.getHeight()){
					if(model.hasObject(WorldModel.COW, x+i, y+j)){
						//If cow is not between (x,y) and corral
						if(xDir*i > 0 || yDir*j > 0)
							p += 6 - Math.abs(i); //Punish harder if it gets closer to cow						
					}
				}
			}			
		}
		
		if(model.hasObject(WorldModel.AGENT, x, y))
			p += 5;
		
		return p;
	}
	
	private int punishmentSpecial(LocalWorldModel model, int x, int y){
		int p = 0;
	
		
		if(model.hasObject(WorldModel.FENCE, x, y))
			p += 5;
		
		if(model.hasObject(WorldModel.AGENT, x, y))
			p += 5;		
		if(model.hasObject(WorldModel.CORRAL, x, y))
			p += 10;
		
		
		return p;
	}
	
    public String getMove(Node from, Node to) {
        if (to.x < from.x && to.y == from.y)
            return "west";
        else if (to.x > from.x && to.y == from.y)
            return "east";
        else if (to.x == from.x && to.y < from.y)
            return "north";
        else if (to.x > from.x && to.y < from.y)
            return "northeast";
        else if (to.x < from.x && to.y < from.y)
            return "northwest";
        else if (to.x == from.x && to.y > from.y)
            return "south";
        else if (to.x > from.x && to.y > from.y)
            return "southeast";
        else if (to.x < from.x && to.y > from.y)
            return "southwest";
        else
            return "skip";
    }
	
    private int getContent(int x, int y, LocalWorldModel model) {
    	if (model.hasObject(WorldModel.CLEAN, x, y))
    		return WorldModel.CLEAN;
    	else if (model.hasObject(WorldModel.AGENT, x, y))
    		return WorldModel.AGENT;
    	else if (model.hasObject(WorldModel.OBSTACLE, x, y))
    		return WorldModel.OBSTACLE;
    	else if (model.hasObject(WorldModel.COW, x, y))
    		return WorldModel.COW;
    	else if (model.hasObject(WorldModel.CORRAL, x, y))
    		return WorldModel.CORRAL;
    	else if (model.hasObject(WorldModel.ENEMY, x, y))
    		return WorldModel.ENEMY;
    	else if (model.hasObject(WorldModel.ENEMYCORRAL, x, y))
    		return WorldModel.ENEMYCORRAL;
    	else if (model.hasObject(WorldModel.FENCE, x, y))
    		return WorldModel.FENCE;
    	else if (model.hasObject(WorldModel.SWITCH, x, y))
    		return WorldModel.SWITCH;
    	
    	return 0;
    }
    
    public boolean isClear(int x, int y, LocalWorldModel model, boolean fenceClear) {
    	int content = getContent(x,y,model);
    	return content != WorldModel.OBSTACLE &&
    		(content != WorldModel.FENCE || fenceClear) &&
    		content != WorldModel.SWITCH &&
    		content != WorldModel.ENEMY;
    }
    
	
	public int getG(){
		return g;
	}
	
	public int hashCode(){
		return (int)Math.pow(2, x) * (int)Math.pow(3, y);
	}
	
	public int getF(){
		return f;
	}
	
	public int getX(){
		return x;
	}
	
	public int getY(){
		return y;
	}
	
	public Heuristic getH(){
		return h;
	}

	
	public boolean equals(Object o){
		if(!(o instanceof Node))
			return false;
		Node n = (Node) o;
		return n.x == this.x && n.y == this.y;
	}
	
} //Node

class Solution {

	private boolean succes;
	private ArrayList<Node> path;
	
	public Solution(){
		path = new ArrayList<Node>();
		succes = false;
	}
	
	public Solution(Node initial, Node terminal){
		succes = true;
		
		path = new ArrayList<Node>();
		
		path.add(terminal);
		Node n = terminal;

		while(!n.equals(initial)){
			n = n.getParent();
			path.add(0, n);
		}	
	}

	public boolean succeded(){
		return succes;
	}
	
	public ArrayList<Node> getPath(){
		return path;
	}
	
	public int getPathLength(){
		return path.size()-1;
	}

    public String getFirstMove() {
    	if(!succes)
    		return "skip";
    	int xFrom = path.get(0).getX();
    	int yFrom = path.get(0).getY();
    	int xTo = path.get(1).getX();
    	int yTo = path.get(1).getY();
    	
        if (xTo < xFrom && yTo == yFrom)
            return "west";
        else if (xTo > xFrom && yTo == yFrom)
            return "east";
        else if (xTo == xFrom && yTo < yFrom)
            return "north";
        else if (xTo > xFrom && yTo < yFrom)
            return "northeast";
        else if (xTo < xFrom && yTo < yFrom)
            return "northwest";
        else if (xTo == xFrom && yTo > yFrom)
            return "south";
        else if (xTo > xFrom && yTo > yFrom)
            return "southeast";
        else if (xTo < xFrom && yTo > yFrom)
            return "southwest";
        else
            return "skip";
    }
}

class Heuristic implements Comparator<Node>{

	private LocalWorldModel model;
	private int endX, endY;
	
	public Heuristic(LocalWorldModel model, int endX, int endY){
		this.model = model;
		this.endX = endX;
		this.endY = endY;
	}
	
	public int heuristic(Node n) {
		return Math.max(Math.abs(n.getX() - endX), Math.abs(n.getY() - endY));
	}

	@Override
	public int compare(Node n0, Node n1) {
		if(n0.getF() < n1.getF()) return -1;
		else if(n0.getF() > n1.getF()) return 1;
		return -1;
	}				
};
